#include "Main.h"

#define convertJoyValue(val) (val)*-1.0

Main::Main()
{
	// DRIVER CONTROL INSTANTIATIONS
	m_joystickDriver1 = new Joystick( JOYSTICK_DRIVER_1_PORT );
	m_joystickDriver2 = new Joystick( JOYSTICK_DRIVER_2_PORT );
	m_joystickOperator1 = new Joystick( JOYSTICK_OPERATOR_1_PORT );
	m_joystickOperator2 = new Joystick( JOYSTICK_OPERATOR_2_PORT );
	
	// SENSOR INSTANTIATIONS
	m_globals = GlobalData::GetInstance();
	
	// ACTUATOR INSTANTIATIONS
	// Drive train
#if TWO_MOTORS
	m_DTLeftA = new Talon( DRIVETRAIN_L_PWM );
	m_DTRightA = new Talon( DRIVETRAIN_R_PWM );	
#else
	m_DTLeftA = new Talon( DRIVETRAIN_L_1_PWM );
	m_DTLeftB = new Talon( DRIVETRAIN_L_2_PWM );
	m_DTRightA = new Talon( DRIVETRAIN_R_1_PWM );
	m_DTRightB = new Talon( DRIVETRAIN_R_2_PWM );
#endif
}
Main::~Main()
{
	delete m_globals;
}

void Main::Autonomous()
{
}
void Main::OperatorControl()
{
	while ( IsOperatorControl() && IsEnabled() )
	{
		Input();
		Process();
		Output();
		printf( "\n\n" );
	}
}
void Main::Disabled()
{
	
}

void Main::Input()
{
	
	// RECEIVE DATA FROM DRIVER CONTROLS
	// Drivetrain
	m_globals->i_joystick_1_x = m_joystickDriver1->GetX();
	m_globals->i_joystick_1_y = m_joystickDriver1->GetY();
	m_globals->i_joystick_2_x = m_joystickDriver2->GetX();
	m_globals->i_joystick_2_y = m_joystickDriver2->GetY();
	m_globals->i_joystickArcadeToggle = m_joystickDriver1->GetRawButton( JOYSTICK_ARCADE_TOGGLE );
}
void Main::Process()
{
	// DRIVE TRAIN
	printf( "JOYSTICK INPUT\n" );
	printf( "x1: %f    y1: %f\n", m_globals->i_joystick_1_x, m_globals->i_joystick_1_y );
	printf( "x2: %f    y2: %f\n", m_globals->i_joystick_2_x, m_globals->i_joystick_2_y );
	
	if ( m_globals->i_joystickArcadeToggle )
	{
		printf( "Arcade\n" );
		if ( fabs( m_globals->i_joystick_2_x - JOYSTICK_CENTER ) < JOYSTICK_DEADBAND )
		{
			// Drive straight
			printf( "Drive straight\n" );
			m_globals->o_drivetrainLeft = convertJoyValue( m_globals->i_joystick_2_y );
			m_globals->o_drivetrainRight = convertJoyValue( m_globals->i_joystick_2_y );
		}
		else if ( fabs( m_globals->i_joystick_2_x - JOYSTICK_CENTER ) > JOYSTICK_DEADBAND && 
				  fabs( m_globals->i_joystick_2_y - JOYSTICK_CENTER ) > JOYSTICK_DEADBAND )
		{
			printf( "Normal arcade " );
			m_globals->o_drivetrainLeft = convertJoyValue( m_globals->i_joystick_2_y ) + convertJoyValue( m_globals->i_joystick_2_y );
			m_globals->o_drivetrainRight = convertJoyValue( m_globals->i_joystick_2_y ) - convertJoyValue( m_globals->i_joystick_2_x );
		}
	}
	else
	{
		printf( "Tank\n" );
		if ( fabs( m_globals->i_joystick_1_x - JOYSTICK_CENTER ) > JOYSTICK_DEADBAND &&
			 fabs( m_globals->i_joystick_1_y - JOYSTICK_CENTER ) > JOYSTICK_DEADBAND )
			m_globals->o_drivetrainLeft = convertJoyValue( m_globals->i_joystick_1_y );
		else
			m_globals->o_drivetrainLeft = 0;
		if ( fabs( m_globals->i_joystick_2_x - JOYSTICK_CENTER ) > JOYSTICK_DEADBAND && 
			 fabs( m_globals->i_joystick_2_y - JOYSTICK_CENTER ) > JOYSTICK_DEADBAND )
			m_globals->o_drivetrainRight = convertJoyValue( m_globals->i_joystick_2_y );
		else
			m_globals->o_drivetrainRight = 0;
	}
	
#if MOTORS_DIRECTION_SYNC
	m_globals->o_drivetrainRight *= -1;
#endif
}

void Main::Output()
{
	// Drive train actuators
	printf( "DRIVETRAIN OUTPUT\n" );
	printf( "Left: %f\n", m_globals->o_drivetrainLeft );
	printf( "Right: %f\n", m_globals->o_drivetrainRight );
#if TWO_MOTORS
	m_DTLeftA->SetSpeed( m_globals->o_drivetrainLeft );
	m_DTRightA->SetSpeed( m_globals->o_drivetrainRight );
#else
	m_DTLeftA->SetSpeed( m_globals->o_drivetrainLeft );
	m_DTLeftB->SetSpeed( m_globals->o_drivetrainLeft );
	m_DTRightA->SetSpeed( m_globals->o_drivetrainRight );
	m_DTRightB->SetSpeed( m_globals->o_drivetrainRight );
#endif
}

START_ROBOT_CLASS(Main);

